#include "drv_algo.h"

/**
 * @brief:    Algo_PID_Init
 * @param:    pPID - PID结构体指针
 * @param:    Kp - 比例系数
 * @param:    Ki - 积分系数
 * @param:    Kd - 微分系数
 * @param:    max_iout - 积分输出限幅（绝对值）
 * @param:    max_out - 总输出限幅（绝对值）
 */
void Algo_PID_Init(algo_pid_t* pPID, float Kp, float Ki, float Kd, float max_iout, float max_out) 
{
    pPID->Kp = Kp;
    pPID->Ki = Ki;
    pPID->Kd = Kd;
    pPID->error_int = 0;
    pPID->last_error = 0;
    pPID->max_iout = max_iout;
    pPID->max_out = max_out;
}


/**
 * @brief:    Algo_PID
 * @param:    pPID - PID结构体指针
 * @param:    actual - 实际值
 * @param:    target - 目标值
 * @retval:   PID输出值
 */
float Algo_PID(algo_pid_t* pPID, float actual, float target) {
  float error = target - actual;
  
  // 积分项计算
  pPID->error_int += error;
  
  // 双向积分限幅
  if (pPID->max_iout > 0) {
    if (pPID->error_int > pPID->max_iout) {
      pPID->error_int = pPID->max_iout;
    } else if (pPID->error_int < -pPID->max_iout) {
      pPID->error_int = -pPID->max_iout;
    }
  }
  
  // PID计算
  float out = pPID->Kp * error + 
              pPID->Ki * pPID->error_int + 
              pPID->Kd * (error - pPID->last_error);
  
  // 双向输出限幅
  if (pPID->max_out > 0) {
    if (out > pPID->max_out) {
      out = pPID->max_out;
      // 抗积分饱和：当输出饱和且误差方向一致时，停止积分
      if (error > 0) {
        pPID->error_int -= error;
      }
    } else if (out < -pPID->max_out) {
      out = -pPID->max_out;
      // 抗积分饱和
      if (error < 0) {
        pPID->error_int -= error;
      }
    }
  }
  
  pPID->last_error = error;
  return out;
}

int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
  /* Converts a float to an unsigned int, given range and number of bits */
  float span = x_max - x_min;
  float offset = x_min;
  return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}

float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
  /* converts unsigned int to float, given range and number of bits */
  float span = x_max - x_min;
  float offset = x_min;
  return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
